#!/usr/bin/env python

import sys
import rospy
from nav_and_digging.srv import BaseNav
from std_msgs.msg import String

import serial
import pynmea2
import time

import math
Pi = 3.1415926
Latitude = 0
Longitude = 0
Heading = 0
Time = 0

LatitudeList = []
LongitudeList = []
HeadingList = []

Cnt = 0

def GpsNav_client(x,y,w):

    try:
        GpsNav_client = rospy.ServiceProxy('/BaseNav_Server', BaseNav)

		# call service
        response = GpsNav_client(x, y, w)
        return response.result
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

def getDistanceByGps(lat1, lng1, lat2, lng2):

    EarthRadius = 6378137
    Rad = Pi / 180.0

    radlat1 = lat1 * Rad
    radlat2 = lat2 * Rad
    a = radlat1 - radlat2
    b = (lng1 - lng2)*Rad
    s = 2*math.asin( \
          math.sqrt(math.pow(math.sin(a/2),2)+ \
          math.cos(radlat1)*math.cos(radlat2)*math.pow(math.sin(b/2),2)) \
          )
    s = s*EarthRadius
    s = (s * 10000) / 10000

    return s



def getAngleByGps(lat1, lng1, lat2, lng2):
    
    x = math.sin(lng2 - lng1) * math.cos(lat2)
    y = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(lng2 - lng1)
    angle = math.atan2(x,y) * 180 / Pi

    if angle<0:
        angle = angle+360

    return angle


def GpsUpdate():
    global Heading, Latitude, Longitude, Time
    ser.flushInput()
    while True:
        recv1 = ser.readline().decode()
        if recv1.startswith('$GNHDT'):
            print('1.$GNHDT Received.')
            record1 = pynmea2.parse(recv1)
            Heading = float(record1.heading)
            print('--------------------------------')
            print('Heading:', Heading)

            recv2 = ser.readline().decode()
            if recv2.startswith('$GPGGA'):
                print('2.$GPGGA Received.')
                record2 = pynmea2.parse(recv2)
                print('Number of Satellites availabe:', record2.num_sats)

                recv3 = ser.readline().decode()
                if recv3.startswith('$GPRMC'):
                    print('3.$GPRMC Received.')
                    record3 = pynmea2.parse(recv3)
                    Latitude = float(record3.latitude)
                    Longitude = float(record3.longitude)
                    Time = str(record3.timestamp)

                    print('Fix Status: ', record3.status)
                    print('Latitude: ', Latitude)
                    print('Longitude: ', Longitude)
                    print('Time: ', Time)
                    break



                    
if __name__ == "__main__":
    
    ser = serial.Serial('/dev/ttyUSB2', 115200, timeout=5)
    while True:
        GpsUpdate()
